 /*************************************************
 *  Object:    Tricopter PID Control (Trunk)
 *  Date:      06/2011 (First)
 *             08/2011 (Last)
 *  Authors:   Teo, Carletto
 *************************************************/

/* Pinout */
#define LED                   PORTC.B2   // LED
#define LED_DIR               TRISC.B2
#define MOTORS_M1             PORTC.B0   // Motor N.1
#define MOTORS_M1_DIR         TRISC.B0
#define MOTORS_M2             PORTC.B1   // Motor N.2
#define MOTORS_M2_DIR         TRISC.B1
#define MOTORS_M3             PORTC.B6   // Motor N.3
#define MOTORS_M3_DIR         TRISC.B6
#define MOTORS_MS             PORTC.B7   // Motor Servo
#define MOTORS_MS_DIR         TRISC.B7
#define RX1                   PORTB.B4   // Receiver CH1
#define RX2                   PORTB.B5
#define RX3                   PORTB.B6   // Receiver CH2
#define RX4                   PORTB.B7
#define RX1_DIR               TRISB.B4   // Receiver CH3
#define RX2_DIR               TRISB.B5
#define RX3_DIR               TRISB.B6   // Receiver CH4
#define RX4_DIR               TRISB.B7
#define DEBUG                 PORTA.B0   // Debug RS232
#define DEBUG_DIR             TRISA.B0

/* Constants */
#define SENSORS_table_dim     16         // Table dimension for accelerometer data
#define PID_err_integral_min  0.01
#define PID_timeslice         0.002 //2ms
#define PID_output_min        0
#define PID_output_max        100
#define PID_Kp                0.1
#define PID_Kd                0.01
#define PID_Ki                0.005
#define MOTORS_top_speed      95
                            
// Variables - Main Thread
unsigned short THREAD_counts;   // Number of timeslices gone (1ms each)
unsigned short THREAD_ready;    // Next timeslice started flag

// Variables - Motors
unsigned short MOTORS_speed[4]; // 0 = M1 speed, 1 = M2 speed, 2 = M3 speed, 3 = MS speed

// Variables - Sensors
int SENSORS_table_x[SENSORS_table_dim];  // Accelerations values queue for X-axis
int SENSORS_table_y[SENSORS_table_dim];  // Accelerations values queue for Y-axis
int SENSORS_table_z[SENSORS_table_dim];  // Accelerations values queue for Z-axis
int SENSORS_acceleration[3];             // Accelerations with LPF: 0 = X, 1 = Y, 2 = Z

// Variables - Radio Receiver
unsigned short RX_values[4];                        // Radio receiver acquired values
unsigned short b_old;
unsigned short b_now;
unsigned short b_and;



/* Smarty function to send on RS232 a char String
 */
void Soft_UART_Write_Text(char* testo) {
  char cnt;
  for (cnt=0; cnt<strlen(testo); cnt++) {
      Soft_UART_Write(testo[cnt]);
  }
}

/* Write a Byte of data to an address of ADXL345
 */
void ADXL345_Wr(unsigned short address, unsigned short in) {
    I2C1_Start();
    I2C1_Wr(0x3A);      // Write mode
    I2C1_Wr(address);
    I2C1_Wr(in);
    I2C1_Stop();
}

/* Read a Byte of data from an address of ADXL345
 */
unsigned short ADXL345_Rd(unsigned short address) {
    unsigned short out;
    I2C1_Start();
    I2C1_Wr(0x3A);      // Write mode
    I2C1_Wr(address);
    I2C1_Stop();
    I2C1_Start();
    I2C1_Wr(0x3B);      // Read Mode
    out = I2C1_Rd(0);
    I2C1_Stop();
    return out;
}

/* Fetch acceleration data from ADXL345
 *   NOTE: Address of 3 integer array
 *   must be provided!
 */
void ADXL345_getAcc(int *acc) {
    unsigned short cnt;
    unsigned short acc_tmp[6];
    char z_string[8];

    I2C1_Start();
    I2C1_Wr(0x3A);      // Write mode
    I2C1_Wr(0x32);
    I2C1_Stop();
    I2C1_Start();
    I2C1_Wr(0x3B);      // Read Mode
    for (cnt=0; cnt<6; cnt++) {
        char ack;
        ack = (cnt<5);
        acc_tmp[cnt] = I2C1_Rd(ack);
    }
    I2C1_Stop();

    acc[0] = acc_tmp[1];
    acc[0] = acc[0] << 8;
    acc[0] |= acc_tmp[0];
    acc[1] = acc_tmp[3];
    acc[1] = acc[1] << 8;
    acc[1] |= acc_tmp[2];
    acc[2] = acc_tmp[5];
    acc[2] = acc[2] << 8;
    acc[2] |= acc_tmp[4];
}

/* Initial fetch of all values for the comparing table
 */
void SENSORS_queue_init() {
     unsigned short cnt;
     int SENSORS_acceleration_now[3];
     for (cnt=0; cnt<SENSORS_table_dim; cnt++) {
         ADXL345_getAcc(&SENSORS_acceleration_now);
         SENSORS_table_x[cnt] = SENSORS_acceleration_now[0];
         SENSORS_table_y[cnt] = SENSORS_acceleration_now[1];
         SENSORS_table_z[cnt] = SENSORS_acceleration_now[2];
         Delay_ms(2);
     }
}

/* Update the table with a newer value deleting the old one
 */
void SENSORS_queue_update() {
     static unsigned short SENSORS_table_pointer;
     int acc_tmp[3];
     if (SENSORS_table_pointer < 0 || SENSORS_table_pointer >= SENSORS_table_dim) SENSORS_table_pointer = 0;
     ADXL345_getAcc(&acc_tmp);
     SENSORS_table_x[SENSORS_table_pointer] = acc_tmp[0];
     SENSORS_table_y[SENSORS_table_pointer] = acc_tmp[1];
     SENSORS_table_z[SENSORS_table_pointer] = acc_tmp[2];
     SENSORS_table_pointer++;
}

/* Filtering the axis value with comparing table
 */
int SENSORS_queue_filter(int *fifo) {
     unsigned short cnt;
     long value = 0;
     for (cnt=0; cnt<SENSORS_table_dim; cnt++) {
         value += fifo[cnt];
     }
     return (int)(value/SENSORS_table_dim);
}

void MOTORS_ctrl() {
     unsigned short cnt;
     unsigned short MOTORS_speed_max;
     MOTORS_speed_max = 0;
     THREAD_counts = 0;
        for(cnt=0; cnt<4; cnt++) {
            if(MOTORS_speed[cnt] > MOTORS_top_speed) MOTORS_speed[cnt] = MOTORS_top_speed;
            MOTORS_speed_max = (MOTORS_speed[cnt] >= MOTORS_speed_max) ? MOTORS_speed[cnt] : MOTORS_speed_max;
        }
        for(cnt=0; cnt<=MOTORS_speed_max; cnt++) {
            if(cnt == MOTORS_speed[0]) MOTORS_M1 = 1;
            if(cnt == MOTORS_speed[1]) MOTORS_M2 = 1;
            if(cnt == MOTORS_speed[2]) MOTORS_M3 = 1;
            if(cnt == MOTORS_speed[3]) MOTORS_MS = 1;
            Delay_us(8);
        }
}

/* ESC startup sequence
 * Sends to ESC throttle down for 4s
 */
void ESC_startup(){
     unsigned short cnt=0;
     while(cnt<200) {
         MOTORS_M1 = 0;
         MOTORS_M2 = 0;
         MOTORS_M3 = 0;
         Delay_ms(1);
         MOTORS_M1 = 1;
         MOTORS_M2 = 1;
         MOTORS_M3 = 1;
         Delay_ms(20);
         cnt++;
     }
     T0CON.TMR0ON=1;
}

/* ESC configuration sequence
 * Sends to ESC throttle extremes
 * (all pulled down and all pulled up)
 */
void ESC_config(){
    unsigned short cnt=0;
    while(cnt<150) {
         MOTORS_M1 = 0;
         MOTORS_M2 = 0;
         MOTORS_M3 = 0;
         Delay_ms(2);
         MOTORS_M1 = 1;
         MOTORS_M2 = 1;
         MOTORS_M3 = 1;
         Delay_ms(20);
         cnt++;
    }
    cnt = 0;
    while(cnt<150) {
         MOTORS_M1 = 0;
         MOTORS_M2 = 0;
         MOTORS_M3 = 0;
         Delay_ms(1);
         MOTORS_M1 = 1;
         MOTORS_M2 = 1;
         MOTORS_M3 = 1;
         Delay_ms(20);
         cnt++;
    }
    Delay_ms(2000);
}

/* SENSORS data reading
 * Read a value from every axis and then
 * let it go between LPF to smooth noise
 */
void SENSORS_update(){
   SENSORS_queue_update();
   SENSORS_acceleration[0] = SENSORS_queue_filter(&SENSORS_table_x);
   SENSORS_acceleration[1] = SENSORS_queue_filter(&SENSORS_table_y);
   SENSORS_acceleration[2] = SENSORS_queue_filter(&SENSORS_table_z);
}

void RX_speedPercent(){
   unsigned short cnt;
   float RX_temp;
   for(cnt=0; cnt<4; cnt++) {
//      if(RX_values[cnt] > 95) RX_values[cnt] = 95;
      //RX_values[cnt] = RX_values[cnt] * 0.0165;    // calc percent
      
      RX_temp = (float)(RX_values[cnt]) * 0.5;    // calc percent
      MOTORS_speed[cnt] = floor(RX_temp);
      //RX_values[cnt] = floor((RX_temp / 10 )%10);  // obtain integer
      //MOTORS_speed[cnt] =  RX_values[cnt];
      //MOTORS_speed[cnt] =  200;
   }
}

/* PID data processing
 * Calculate proportional, intergrative and derivative error
 * on each axis to get motors speed
 */
void PID_update() {
    static float PID_err_proportional_old[3];
    static float PID_err_integral[3];
    float PID_err_proportional[3];
    float PID_err_derivative[3];
    int PID_output[3];
    unsigned short cnt;

    for (cnt=0; cnt<3; cnt++) {
        //Caculate PID errors
        PID_err_proportional[cnt] = RX_values[cnt] - SENSORS_acceleration[cnt];  // Need to be implemented conversion between RX and SENSORS values!!!
        if(abs(PID_err_proportional[cnt]) > PID_err_integral_min) PID_err_integral[cnt] += PID_err_proportional[cnt] * PID_timeslice;
        PID_err_derivative[cnt] = (PID_err_proportional[cnt] - PID_err_proportional_old[cnt]) / PID_timeslice;
        PID_output[cnt] = (int)( (PID_Kp*PID_err_proportional[cnt]) + (PID_Ki*PID_err_integral[cnt]) + (PID_Kd*PID_err_derivative[cnt]) );
        //Saturation Filter
        if(PID_output[cnt] > PID_output_max) PID_output[cnt] = PID_output_max;
        else if(PID_output[cnt] < PID_output_min) PID_output[cnt] = PID_output_min;
        //Update error
        PID_err_proportional_old[cnt] = PID_err_proportional[cnt];
    }
    MOTORS_speed[0] = PID_output[1] + PID_output[2] - PID_output[0]; // M1 = -X  +Y  +Z
    MOTORS_speed[1] = PID_output[2] - PID_output[1] - PID_output[1]; // M2 =     -2Y +Z
    MOTORS_speed[2] = PID_output[0] + PID_output[1] + PID_output[2]; // M3 = +X  +Y  +Z
}

unsigned short oldLineState[4];


/* MAIN LOOP
 */
void main() {

    /* Pinout direction assignment */
    LED_DIR = 0;         // LED is set as OUTPUT
    MOTORS_M1_DIR = 0;   // M1 is set as OUTPUT
    MOTORS_M2_DIR = 0;   // M2 is set as OUTPUT
    MOTORS_M3_DIR = 0;   // M3 is set as OUTPUT
    MOTORS_MS_DIR = 0;   // MS is set as OUTPUT
    RX1_DIR = 1;
    RX2_DIR = 1;
    RX3_DIR = 1;
    RX4_DIR = 1;
    LED = 0;
    
    /* Registers initialization */
    ADCON1 = 0x0F;    // Set A/D Disabled
    CMCON = 0xCF;     // Set Comparator Disabled
    

    
    /* Periphals initialization */
    DEBUG_DIR = 0;
    Soft_UART_Init(&PORTA, 1, 0, 57600, 0);
    I2C1_Init(400000);
    MOTORS_M1 = 1;
    MOTORS_M2 = 1;
    MOTORS_M3 = 1;
    MOTORS_MS = 1;
    Delay_ms(500);
    

    /* ADXL345 initialization */
    ADXL345_Wr(0x2C, 0x0C);      // 400Hz conversion data rate
    ADXL345_Wr(0x2D, 0x08);      // Measuring activated ;)
    ADXL345_Wr(0x31, 0x0B);      // Resolution set to +/- 16g (WOW!)
    Delay_ms(500);

    /* Timer0 initialization */
    T0CON.T0PS0 = 0;     //Prescaler 1:64
    T0CON.T0PS1 = 1;
    T0CON.T0PS2 = 1;
    T0CON.PSA = 1;       // Timer Clock Source is from Prescaler
    T0CON.T0CS = 0;      // Prescaler gets clock from FCPU
    T0CON.T08BIT = 1;    // 8 BIT MODE
    INTCON.TMR0IE = 0;   // Enable TIMER0 Interrupt
    TMR0L = 0;           // TMR0 preset
    TMR0H = 0;           // TMR0 preset
    INTCON.TMR0IF = 0;
    T0CON.TMR0ON = 0;
    
    /* Timer1 initialization */
    T1CON.T1CKPS1 = 0;        //prescaler 1:0
    T1CON.T1CKPS0 = 0;        //prescaler 1:0   --> 0,33uS per incremento
    INTCON.GIE = 1 ;         //enable global interrupts
    INTCON.PEIE = 1 ;        //PEIE: Peripheral Interrupt Enable bit
    PIE1.TMR1IE = 1  ;        //enable timer 1 interrupt
    TMR1H = 0xD1;            //High byte timer = 0
    TMR1L = 0x80;            //Low  byte timer = 0
    PIR1.TMR1IF = 0 ;        //clear timer 1 interrupt flag
    T1CON.TMR1ON = 1;        //timer 1 start/stop

    /** External Interrupt setup 4 reciver   **/
    INTCON.RBIE = 1; //RB Port Change Interrupt Enable bit (1)
    INTCON.RBIF = 0; //RB Port Change Interrupt Flag bit
    INTCON.INT0IE = 0; //INT0 External Interrupt Enable bit  (1)
    INTCON.INT0IF = 0; //INT0 External Interrupt Flag bit
    

    /* Variables initialization */
    THREAD_counts = 0;
    THREAD_ready = 0;
    MOTORS_speed[0] = 0;
    MOTORS_speed[1] = 0x20;
    MOTORS_speed[2] = 0x30;
    MOTORS_speed[3] = 0x60;
    b_old = PORTB;
    b_now = PORTB;
    

    /* Startup */
    SENSORS_queue_init();                  // ADXL345 Array filling up
    ESC_config();
    ESC_startup();
    
    T0CON.TMR0ON = 1;
    
    while (1) {
        if(THREAD_ready){
             switch(THREAD_counts){
                  case 1:
                      LED=!LED;
                      THREAD_ready = 0;
                      break;
             
                  case 20:
                       MOTORS_M1 = 0;
                       MOTORS_M2 = 0;
                       MOTORS_M3 = 0;
                       MOTORS_MS = 0;
                       THREAD_ready = 0;
                       break;
                  case 0:
                  case 2:
                  case 4:
                  case 6:
                  case 8:
                  case 10:
                  case 12:
                  case 14:
                  case 16:
                  case 18:
                      RX_speedPercent();
                      SENSORS_update();
                      //PID_update();
                      THREAD_ready = 0;
                      break;

                  case 21:
                      MOTORS_ctrl();
                      THREAD_ready = 0;
                      break;
                  case 3:
                  case 5:
                  case 7:
                  case 9:
                  case 11:
                  case 13:
                  case 15:
                  case 17:
                  case 19:
                       THREAD_ready = 0;
                       break;

             }
        }
    }
}


void interrupt() {

     if(INTCON.RBIF){
          INTCON.RBIE = 0;
          b_now = PORTB;
          b_and = b_now ^ b_old;

          // Falling edge - Starting count
          if(b_and & 0x010) {
              if(b_now & 0x010) {   // Rising  (End)
                  RX_values[0] = (TMR0L - RX_values[0]);
                  //Soft_UART_Write(TMR0L);
                  //Soft_UART_Write(RX_values[0]);
              } else {             // Falling (Start)
                  //Soft_UART_Write(TMR0L);
                  RX_values[0] = TMR0L;
              }
          }
          if(b_and & 0x020) {
              if(b_now & 0x020) {
                  RX_values[1] = TMR0L - RX_values[1];
              } else {
                  RX_values[1] = TMR0L;
              }
          }
          if(b_and & 0x040) {
              if(b_now & 0x040) {
                  RX_values[2] = TMR0L - RX_values[2];
              } else {
                  RX_values[2] = TMR0L;
              }
          }
          if(b_and & 0x080) {
              if(b_now & 0x080) {
                  RX_values[3] = TMR0L - RX_values[3];
              } else {
                  RX_values[3] = TMR0L;
              }
          }
          
          b_old = b_now;
          INTCON.RBIF = 0; // Try to remove it to save time
          INTCON.RBIE = 1;
     }//end RBIF interrupt
     
     if(PIR1.TMR1IF) {
          TMR1H = 0xD1;
          TMR1L = 0x80;
          PIR1.TMR1IF=0;
          THREAD_counts++;
          THREAD_ready = 1;
     }//end TMR1 interrupt
     
     /*if(INTCON.TMR0IF){
       INTCON.TMR0IF=0;
       LED=!LED;
     }    */
     
     
 }